#include "joystick_device.h"
#include "qlogging.h"

JoystickDevice::JoystickDevice(QObject *parent) : QObject{parent} {}

JoystickDevice::~JoystickDevice()
{
    if (serialPort != nullptr && serialPort->isOpen())
    {
        serialPort->clear();
        serialPort->close();
        serialPort->deleteLater();
    }
}

bool JoystickDevice::InitDevice(const QString &device, int baudRate)
{
    // Open serial port
    serialPort = new QSerialPort(this);
    serialPort->setPortName(device);
    serialPort->setBaudRate(baudRate);
    serialPort->setDataBits(QSerialPort::Data8);
    serialPort->setParity(QSerialPort::NoParity);
    serialPort->setStopBits(QSerialPort::OneStop);

    serialPort->open(QIODevice::ReadOnly);
    if (!serialPort->isOpen())
    {
        qDebug("open serial port failed");
        return false;
    }

    // timer = new QTimer(this);
    // timer->setInterval(10);
    // connect(timer, &QTimer::timeout, this, &JoystickDevice::ReadData);
    // timer->start();

    connect(serialPort, &QSerialPort::readyRead, this, &JoystickDevice::ReadData);
    return true;
}
// leftx, lefty, rightx, righty by (mid,halfRange)
void JoystickDevice::SetRemapParam(QVector<float> params)
{
    if (params.size() != 8)
    {
        qDebug("joystick device remap params size error");
    }
    this->params = params;
}

float JoystickDevice::ExtractValue(const QByteArray &slice, float mid, float halfRange)
{
    unsigned int data = static_cast<unsigned int>(slice[0]) | (static_cast<unsigned int>(slice[1]) << 8) |
                        (static_cast<unsigned int>(slice[2]) << 16) | (static_cast<unsigned int>(slice[3]) << 24);
    float *fv = reinterpret_cast<float *>(&data);
    return (*fv - mid) / halfRange;
}

bool JoystickDevice::CheckFrame(const QByteArray &frame)
{
    if (frame[1] == 0x03)
    {
        return false;
    }
    unsigned char code = 0;
    for (int i = 0; i < frame.size() - 1; i++)
    {
        code += frame[i];
    }
    code = ~code;
    return code == frame[frame.size() - 1];
}

void JoystickDevice::DecodeFrame(const QByteArray &frame)
{
    float value1 = ExtractValue(frame.mid(3, 4), params[4], params[5]);
    float value2 = ExtractValue(frame.mid(8, 4), params[2], params[3]);
    float value3 = ExtractValue(frame.mid(13, 4), params[0], params[1]);
    float value4 = ExtractValue(frame.mid(18, 4), params[6], params[7]);
    QVector<float> values{value3, value2, value1, value4};
    emit OnInput(values);
}

void JoystickDevice::ReadData()
{
    QByteArray receive = serialPort->readAll();
    for (auto byte : receive)
    {
        buffer.append(byte);
        if (buffer.size() > 1 && static_cast<unsigned char>(buffer[1]) == buffer.size() - 2)
        {
            if (CheckFrame(buffer))
            {
                DecodeFrame(buffer);
            }
            else
            {
                qDebug("Joystick device frame error");
            }
            buffer.clear();
        }
    }
}